/****************************************************************************************
**  File: Math.hpp
**  Author: Asteroth
**  Date: 21/03/2009
****************************************************************************************/

#ifndef __MATH_HPP__
#define __MATH_HPP__

const double PI = 3.1415926535897932384626433832795;
const double PIm2 = 2.0*PI;
const double PId2 = PI/2.0;
const double PId3 = PI/3.0;
const double PId4 = PI/4.0;
const double PId6 = PI/6.0;
const double DEGTORAD = PI/180.0;
const double RADTODEG = 180.0/PI;

#define INF_VECTOR3 		Vector3x<float>(INFINITY, INFINITY, INFINITY)
#define ZERO_VECTOR3 		Vector3x<float>(0.0f, 0.0f, 0.0f)

class Math {
public:
	// === Min/Max =============================================================
	// minor between two values
	template <typename T>
	static inline T min(const T &val1, const T &val2) {
		return val1<val2 ? val1 : val2;
	}
	// major between two values
	template <typename T>
	static inline T max(const T &val1, const T &val2) {
		return val1<val2 ? val2 : val1;
	}
	// minor between three values
	template <typename T>
	static inline T min(const T &val1, const T &val2, const T &val3) {
		T m = val1<val2 ? val1 : val2;
		return m<val3 ? m : val3;
	}
	// major between three values
	template <typename T>
	static inline T max(const T &val1, const T &val2, const T &val3) {
		T m = val1<val2 ? val2 : val1;
		return m<val3 ? val3 : m;
	}

	// === Length ==============================================================
	// length of a quaternion
	template <typename T>
	static inline T length(const Quaternionx<T> &quat)
	{
		return (T)sqrt((double)(quat.X*quat.X + quat.Y*quat.Y + quat.Z*quat.Z + quat.W*quat.W));
	}
	// length of a 3d vector
	template <typename T>
	static inline T length(const Vector3x<T> &vec) {
		return (T)sqrt((double)(vec.X*vec.X + vec.Y*vec.Y + vec.Z*vec.Z));
	}
	// length of a 2d vector
	template <typename T>
	static inline T length(const Vector2x<T> &vec) {
		return (T)sqrt((double)(vec.X*vec.X + vec.Y*vec.Y));
	}
	// length of a 3d segment
	template <typename T>
	static inline T length(const Segment3x<T> &seg) {
		return Length(seg.B - seg.A);
	}
	// length of a 2d segment
	template <typename T>
	static inline T length(const Segment2x<T> &seg) {
		return Length(seg.B - seg.A);
	}

	// === Normal ==============================================================
	// normal of a 3d vector
	template <typename T>
	static inline Vector3x<T> normal(const Vector3x<T> &vec)
	{
		float len = sqrt(vec.X*vec.X + vec.Y*vec.Y + vec.Z*vec.Z);
		if(len>0)
			return Vector3x<T>(vec.X/len, vec.Y/len, vec.Z/len);
		else
			return vec;
	}
	// normal of a 2d vector
	template <typename T>
	static inline Vector2x<T> normal(const Vector2x<T> &vec)
	{
		float len = sqrt(vec.X*vec.X + vec.Y*vec.Y);
		if(len>0)
			return Vector2x<T>(vec.X/len, vec.Y/len);
		else
			return vec;
	}

	// === DotProduct ==========================================================
	// dot product of two quaternions
	template <typename T>
	static inline T dotProduct(const Quaternionx<T> &quat1, const Quaternionx<T> &quat2)
	{
		return quat1.X*quat2.X + quat1.Y*quat2.Y + quat1.Z*quat2.Z + quat1.W*quat2.W;
	}
	// dot product of two 3d vectors
	template <typename T>
	static inline T dotProduct(const Vector3x<T> &vec1, const Vector3x<T> &vec2)
	{
		return vec1.X*vec2.X + vec1.Y*vec2.Y + vec1.Z*vec2.Z;
	}
	// dot product of two 2d vectors
	template <typename T>
	static inline T dotProduct(const Vector2x<T> &vec1, const Vector2x<T> &vec2)
	{
		return vec1.X*vec2.X + vec1.Y*vec2.Y;
	}

	// === CrossProduct ========================================================
	// cross product of two 3d vectors
	template <typename T>
	static inline Vector3x<T> crossProduct(const Vector3x<T> &vec1, const Vector3x<T> &vec2)
	{
		return Vector3x<T>(
			vec1.Y * vec2.Z - vec1.Z * vec2.Y,
			vec1.Z * vec2.X - vec1.X * vec2.Z,
			vec1.X * vec2.Y - vec1.Y * vec2.X);
	}

	// === Inverse =============================================================
	// inverse of 2d vector
	template <typename T>
	static inline Vector2x<T> inverse(const Vector2x<T> &vec)
	{
		return Vector2x<T>(-vec.X, -vec.Y);
	}
	// inverse of 3d vector
	template <typename T>
	static inline Vector3x<T> inverse(const Vector3x<T> &vec)
	{
		return Vector3x<T>(-vec.X, -vec.Y, -vec.Z);
	}
	// inverse of quaternion
	template <typename T>
	static inline Quaternionx<T> inverse(const Quaternionx<T> &quat)
	{
		return Quaternionx<T>(-quat.X, -quat.Y, -quat.Z, quat.W);
	}

	// === ToPlane =============================================================
	// transform a triangle to plane
	template <typename T>
	static inline Planex<T> toPlane(const Triangle3x<T> &triangle)
	{
		return Planex<T> (triangle.V1, normal(crossProduct(triangle.V2 - triangle.V1, triangle.V3 - triangle.V1)));
	}

	// === Distance ============================================================
	// distance between a 3d point and a 3d infinite line
	template <typename T>
	static inline T distance(const Vector3x<T> &point, const Line3x<T> &line)
	{
		return Length(crossProduct(Normal(line.N), (point - line.P)));
	}
	// distance between a 2d point and a 2d infinite line
	template <typename T>
	static inline T distance(const Vector2x<T> &point, const Line2x<T> &line)
	{
		return Length(crossProduct(Vector3(normal(line.N), 0), Vector3(point - line.P, 0)));
	}
	// distance between two 3d points
	template <typename T>
	static inline T distance(const Vector3x<T> &point1, const Vector3x<T> &point2)
	{
		return Length(point2 - point1);
	}
	// distance between two 2d points
	template <typename T>
	static inline T distance(const Vector2x<T> &point1, const Vector2x<T> &point2)
	{
		return Length(point2 - point1);
	}

	// === Area ================================================================
	// area of a 3d triangle
	template <typename T>
	static inline T area(const Triangle3x<T> &tri)
	{
		return 0.5 * length(crossProduct(tri.V2 - tri.V1, tri.V3 - tri.V1));
	}

	// === Angle ===============================================================
	// angle between two 3d vectors
	template <typename T>
	static inline T angle(const Vector3x<T> &vec1, const Vector3x<T> &vec2)
	{
		return (T)acos(dotProduct(normal(vec1), normal(vec2)));
	}
	// angle between two 2d vectors
	template <typename T>
	static inline T angle(const Vector2x<T> &vec1, const Vector2x<T> &vec2)
	{
		return (T)acos(dotProduct(normal(vec1), normal(vec2)));
	}

	// === Inside ==============================================================
	// is the point inside the triangle?
	// (assuming that the point is on the plane of the triangle)
	template <typename T>
	static inline bool inside(const Vector3x<T> &point, const Triangle3x<T> &triangle)
	{
		Vector3x<T> u = triangle.V2 - triangle.V1;
		Vector3x<T> v = triangle.V3 - triangle.V1;

		T a = dotProduct(u, u);
		T b = dotProduct(u, v);
		T c = dotProduct(v, v);

		Vector3x<T> vp = point - triangle.V1;
		T d = dotProduct(vp, u);
		T e = dotProduct(vp, v);

		T x = (d*c)-(e*b);
		T y = (e*a)-(d*b);
		T ac_bb = (a*c)-(b*b);
		T z = x+y-ac_bb;

		return (( ((unsigned int&)(z)) & ~(((unsigned int&)(x))|((unsigned int&)(y))) ) & 0x80000000)!=0;
	}
	// is the point inside the (axis parallel) box
	template <typename T>
	static inline bool inside(const Vector3x<T> &point, const Boxx<T> &box)
	{
		return (point.X>=box.Minor.X && point.X<=box.Major.X
			&& point.Y>=box.Minor.Y && point.Y<=box.Major.Y
			&& point.Z>=box.Minor.Z && point.Z<=box.Major.Z);
	}

	// === Intersection ========================================================
	// intersection position between a 3d infinite line and a plane
	// (INFINITE_VECTOR3 for parallel or non intersection)
	template <typename T>
	static inline Vector3x<T> intersection(const Line3x<T> &line, const Planex<T> &plane)
	{
		T r = dotProduct(normal(plane.N), normal(line.D));
		if(r<0.00001 && r>-0.00001)
			return INF_VECTOR3;

		r = dotProduct(normal(plane.N), plane.P - line.P) / r;

		return Vector3x<T>(line.P + (normal(line.D) * r));
	}
	// intersection position between a 3d segment and a plane
	// (INFINITE_VECTOR3 for non intersection)
	template <typename T>
	static inline Vector3x<T> intersection(const Segment3x<T> &segment, const Planex<T> &plane)
	{
		Vector3x<T> intersect = intersection(Line3x<T>(segment.A, normal(segment.B - segment.A)), plane);
		if(inside(intersect, Boxx<T>(segment.A, segment.B)))
			return intersect;

		return INF_VECTOR3;
	}
	// intersection position between a 3d infinite line and a 3d triangle
	// (INFINITE_VECTOR3 for non intersection)
	template <typename T>
	static inline Vector3x<T> intersection(const Line3x<T> &line, const Triangle3x<T> &triangle)
	{
		Vector3x<T> intersect = intersection(line, Planex<T>(triangle.V1, crossProduct(triangle.V3 - triangle.V1, triangle.V2 - triangle.V1)));
		if(inside(intersect, triangle))
			return intersect;

		return INF_VECTOR3;
	}
	// intersection position between a 3d segment and a 3d triangle
	// (INFINITE_VECTOR3 for non intersection)
	template <typename T>
	static inline Vector3x<T> intersection(const Segment3x<T> &segment, const Triangle3x<T> &triangle)
	{
		Vector3x<T> intersect = intersection(Line3x<T>(segment.A, normal(segment.B - segment.A)), triangle);
		if(inside(intersect, Boxx<T>(segment.A, segment.B)))
			return intersect;

		return INF_VECTOR3;
	}
} math;

#endif
